void setup(){
//  HardwareSerialBuf SerialBuf;
  SerialBuf.begin(115200);

  pinMode(8,INPUT);    // Rx Radio Input
  pinMode(5,OUTPUT);   // Red LED
  pinMode(6,OUTPUT);   // BLue LED
  pinMode(7,OUTPUT);   // Yellow LED

  // Offset values for accels and gyros
  AN_OFFSET[0] = gyro_offset_roll*4;
  AN_OFFSET[1] = gyro_offset_pitch*4;
  AN_OFFSET[2] = gyro_offset_yaw*4;
  AN_OFFSET[3] = acc_offset_x*4;
  AN_OFFSET[4] = acc_offset_y*4;
  AN_OFFSET[5] = acc_offset_z*4;

  // ADC initialization
  Analog_Reference(EXTERNAL);
  Analog_Init();

  // Take the gyro offset values
  /* Serial.println("Calibration des gyros");
   
   for(int i=0;i<150;i++) //read 150 times
   {
   Read_adc_raw();
   for(int y=0; y<=2; y++)   // Read initial ADC values for gyros offset
   AN_OFFSET[y] = AN_OFFSET[y]*0.8 + AN[y]*0.2;
   delay(20);  //read at 50 Hz
   }
   
   Serial.print("AN0:");
   Serial.print(AN_OFFSET[0]);
   Serial.print(",AN1:");
   Serial.print(AN_OFFSET[1]);
   Serial.print(",AN2:");
   Serial.print(AN_OFFSET[2]);  
   Serial.print(",AN3:");
   Serial.print(AN_OFFSET[3]);
   Serial.print (",AN4:");
   Serial.print(AN_OFFSET[4]);
   Serial.print (",AN5:");
   Serial.print(AN_OFFSET[5]);
   Serial.print (",");
   
   Serial.println("Ready to fly");*/
  digitalWrite(7,HIGH); // Light yellow led
}







